#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "kuruma.h"
#include "OLED.h"
#include "behavior.h"
#include "command.h"
#include "HMC5883L.h"
#include <math.h>

HMC5883L sensor;

extern float mag_heading;  // 原始磁力计航向角
extern int state;
extern float distance;
extern float direction;
extern int state1;
extern float sub_distance;
int main(void)
{
	NVIC_Configuration();        //一定要有这个，否则会导致中断优先级失效，甚至使9600以上串口无法正常接收数据（中断）
	delay_init();	    	     //延时函数初始化	  
	micros_init();
	
	OLED_Init();
	OLED_ShowString(1,1,"hello");
	
	serial_begin(SERIAL1, 115200);
	serial_begin(SERIAL2, 9600);
	
	serial_println(SERIAL1,"Kuruma begining...");
	
	gpio_init_debug_pins_as_gpio(); //将PB3，PB4，PA15由jtag引脚变成可用的引脚

	kuruma_init();   //小车初始化
	behavior_init(); //小车基本行为
	
	kuruma_enable();
	
	int command_sent = 0;
	int t = 0;
	while(1) {
		command_sent = 0;
		while(serial_available(SERIAL2)) { //来自蓝牙的数据
			int c = serial_read(SERIAL2);
			command_update(c); //同时进行解析
			command_sent = 1;
		}
		if(!command_sent) {
			command_update(0);
		}
		while(serial_available(SERIAL1)) {
			int c = serial_read(SERIAL1);
			serial_write(SERIAL2,c);
		}
		t++;
		if(t == 30) {
			odometry_print();
			printf("azimuth=%.1f°\n", 
				   odom.azimuth * 180.0f / M_PI);
			if(odom.mpu6050_error) {
				printf("MPU6050 ERROR\n");
				odom.mpu6050_error = 0;
				//kuruma_disable();
			}
			if(odom.hmc5883l_error) {
				printf("HMC5883l ERROR\n");
				odom.hmc5883l_error = 0;
				//kuruma_disable();
			}
//			if(!odom.mpu6050_error && !odom.hmc5883l_error) {
//				//printf("Sensors recovered\n");
//				kuruma_enable();
//			}
			printf("state = %d, state1 = %d, dis = %.2f,sub_dis = %.2f, dir = %.1f\n",state,state1,distance,sub_distance,direction*RAD_TO_DEG);
			//printf("theta by gyro:%.1f, theta by mag: %.1f\n",odom.theta * 180.0f / M_PI,(odom.azimuth-odom.origin_azimuth) * 180.0f / M_PI);
			t = 0;
		}
		delay_ms(20);
	}
}


void test_gyro_drift() {
    printf("=== Gyro Drift Test (keep still!) ===\n");
    delay_ms(2000);

    float integral = 0.0f;
    unsigned long prev_time = micros();

    for (int i = 0; i < 1000; i++) {
        unsigned long now_us = micros();
        float dt = (now_us - prev_time) / 1000000.0f;
        prev_time = now_us;
        float gz = MPU6050_getGz_dps(&mpu);  // °/s
        integral += gz * dt;
        if (i % 50 == 0) {
            printf("Time:%6dms | Gz:%7.3f°/s | Integrated Angle:%7.3f°\n",
                   i*50, gz, integral);
        }
        delay_ms(50);
    }
}

//	while (1) {
//        float ax, ay, az, gx, gy, gz, temp;
//		printf("%d\n",micros()/1000);
//        if (MPU6050_getMotion(&mpu, &ax, &ay, &az, &gx, &gy, &gz, &temp) == 0) {
//            printf("Ax:%.3f Ay:%.3f Az:%.3f | Gx:%.1f Gy:%.1f Gz:%.1f | T:%.1f°C\n",
//                   ax, ay, az, gx, gy, gz, temp);
//			printf("ax: %.3f\n",MPU6050_getAx_g(&mpu));
//			
//			printf("gz: %.3f\n",MPU6050_getGz_dps(&mpu));
//        }
//        delay_ms(100);
//    }
	//test_gyro_drift();
